// Class include
#include "CNRFReader.hpp"

// System includes
#include <unistd.h>     // For sleep
#include <string.h>
#include <stdio.h>

// Other includes
#include "CTrace.hpp"
#include "Types.hpp"

// Forward declaration includes
#include "CCommand.hpp"
#include "CObserver.hpp"

CNRFReader::CNRFReader(CObserver* pObserver)
: CObservable()
, mRadio("/dev/spidev0.0",8000000,25 /*22*/)  // TODO: get rid of hardcoded values
{
   addObserver(pObserver);

   mRadio.begin();
   mRadio.setRetries(15,15);
   mRadio.setDataRate(RF24_1MBPS);
   //mRadio.setPayloadSize(5);
   mRadio.setChannel(100);
   mRadio.openReadingPipe(1,0xF0F0F0F01ELL);
   mRadio.openWritingPipe(0xF0F0F0F0D2LL);
   mRadio.startListening();

   TRC_DEBUG("Radio is configured");
}

CNRFReader::~CNRFReader()
{
}

void CNRFReader::start()
{
   TRC_DEBUG("Started NRF thread");

   while(true)
   {
      unsigned char* pBuf = 0;
      
      uint8_t bytesRead;

      //pthread_mutex_lock(&mLock);
      //bytesRead = mpSerial->recv(pBuf, COMMAND_LENGTH);

      if (mRadio.available())
      {
         // read from radio until payload size is reached
         bytesRead = mRadio.getDynamicPayloadSize();
         pBuf = new unsigned char[bytesRead];
         mRadio.read(pBuf, bytesRead);
      }
      else
      {
         continue;
      }
      //pthread_mutex_unlock(&mLock);

      if (bytesRead <= 0)
      {
         if (bytesRead < 0)
         {
            TRC_ERROR("Read from Arduino %d bytes", bytesRead);
            usleep(1000*1);
         }
         continue;
      }

/*      
      if (bytesRead != COMMAND_LENGTH)
      {
         TRC_WARNING("Something is wrong while reading command from NRF. Expected %d bytes, but read %d bytes", COMMAND_LENGTH, bytesRead);
         for (int i=0; i<bytesRead; i++) printf("[%d] = %d ", i, pBuf[i]); printf("\n");  // Debug output

         delete[] pBuf;
         continue;
      }
*/

      CCommand cmd(COMMAND_SOURCE_ARDUINO, pBuf);
      TRC_DEBUG("Read command from Arduino: %s, value=%d", toString(cmd.getType()), cmd.getValue32());
      for (int i=0; i<bytesRead; i++) printf("[%d] = %d ", i, pBuf[i]); printf("\n");  // Debug output

      if (COMMAND_INVALID != cmd.getType())
      {
         notifyObservers(cmd);
      }

      delete[] pBuf;
   }
}

void CNRFReader::sendCommand(CCommand cmd)
{
   if (false == isConnected())
   {
      TRC_WARNING("Arduino is closed. Skipping command from Socket to Arduino");
      return;
   }

/*
   if (0 == mpSerial)
   {
      TRC_ERROR("Serial port is NULL. Something is wrong");
      return;
   */

   unsigned char* pRawBuffer = new unsigned char[COMMAND_LENGTH];
   cmd.getRawBuffer(pRawBuffer);

   mRadio.stopListening();
   usleep(100);

   bool res = mRadio.write(pRawBuffer, COMMAND_LENGTH);
   if (false == res)
   {
      TRC_WARNING("Error during sending command");
      //mRadio.printDetails();
   }

   mRadio.startListening();
   usleep(100);
/*
   unsigned int bytesSent = mpSerial->send(pRawBuffer, COMMAND_LENGTH);
   if (bytesSent != COMMAND_LENGTH)
   {
      TRC_WARNING("Not all bytes where sent: %d instead of %d", bytesSent, COMMAND_LENGTH);
   }
   */

   delete[] pRawBuffer;
}

bool CNRFReader::isConnected()
{
   // Assuming that arduino is always connected
   return true;
}